#include "MaxwellVFEM.h"

extern int Edge_end[6][2];                   // in local index

void swap(double complex *a,int *b,int i,int j) 
{
  double complex tmp=a[i];
  a[i]=a[j];
  a[j]=tmp;
  int tmp2=b[i];
  b[i]=b[j];
  b[j]=tmp2;
}

int Random(int i, int j) 
{
  return (i + rand() % (j-i+1));
}

void quicksort(double complex *a,int *b, int left, int right) 
{

  int last=left,i;
  if(left>=right){
    return;
  }
  swap(a,b,left,Random(left,right));
  for(i=left+1;i<=right;i++){
     if(b[i]<b[left]){
       swap(a,b,++last,i);
     }
  }
  swap(a,b,left,last);
  quicksort(a,b,left,last-1);
  quicksort(a,b,last+1,right);
}

void MaxwellVFEM(double complex *A_EM, double complex *phi, 
		 double complex *Jc, double complex *rho, 
		 double complex omega, 
		 double complex *epsilon, double *mu, 
		 int N_node, int N_edge, 
		 int N_triangle, int N_triangle_interface, int N_element, 
		 double *Node, int *Edge, int *Triangle, int *Element, 
		 double *Length_of_Edge,  int *Triangle_Edge, 
		 int *Element_Edge, int *Triangle_Element)

/**********************************
Finite element for Maxwell equations
in Scalar and Vector potentials

\nabla x 1/mu \nabla x A 
- iwe/c \nabla phi -w^2/c^2 A = 4 pi/c J
\nabla \cdot {e(-\nabla phi + iw/c A)} = 4 pi rho

given IBC

***********************************/ 
{ 
  int i, j, m, n;
  int i1, i2, j1, j2;
  int ii, jj, e, ie;
  int im ,jm;
  int isize;
  int icheck;
  int ix,iy,iz;
  int eg1, eg2, eg3, eg4, eg5, eg6;
  FILE *fout1;
  double x0, y0, z0;
  double x1, y1, z1;
  double x2, y2, z2;
  double gaugepara=1;
  double inter, maxm;  
  
  /**********************************
  Element matrix  K, trial vector b
  **********************************/ 
  //printf("Set up Stiff Matrix and RHS ******\n");

  int Msize=N_edge+N_node;     //degree of freedom
  int nz_row=250;              //nonzeros of each row
  double complex *val_temp=(double complex *)malloc(nz_row*Msize*sizeof(double complex)); 
  int *col_ind_temp=(int *)malloc(nz_row*Msize*sizeof(int));
  int *val_size=(int *)malloc(Msize*sizeof(int));
  int *row_ptr=(int *)malloc((Msize+1)*sizeof(int));
  double complex *b_trial=(double complex *)malloc(Msize*sizeof(double complex)); 
 
  for(ix=0;ix<Msize*nz_row;ix++){
     val_temp[ix]=0; 
     col_ind_temp[ix]=-1; 
  }
  for(ix=0;ix<Msize;ix++){ 
     b_trial[ix]=0;
     row_ptr[ix]=0;
     val_size[ix]=0;
  }   
  row_ptr[Msize]=0; 
  int ik=0, p_start=0, p_end=0;  
  
  /**********************************
  a, b, c, d  for linear element
  coefficients of linear element
  a+bx+cy+dz
  ***********************************/
  double ae[4]={0}, be[4]={0}, ce[4]={0}, de[4]={0};

  /**********************************
  x, y, z, for linear element
  coordinates of each vertex
  ***********************************/
  double xe[4]={0}, ye[4]={0}, ze[4]={0}; 
  
  /***********************************
  element volume Vole=|Ve|
  ************************************/
  double Ve=0; 
  double Vole=0;

  /*****************************
  Local matrix and trial vector
  *****************************/ 
  double complex ke1[6][6]={{0}};  // edge edge 
  double complex kemu1[6][6]={{0}};  // edge edge
  double complex ke2[6][4]={{0}};  // edge node
  double complex keeps2[6][4]={{0}};  // edge node
  double complex ke3[6][6]={{0}};  // edge edge
  double complex keeps3[6][6]={{0}};  // edge edge
  double complex ke4[4][4]={{0}};  // node node
  double complex keeps4[4][4]={{0}};  // node node
  double complex ke5[4][6]={{0}};  // node edge
  double complex keeps5[4][6]={{0}};  // node edge
  double complex ke6[4][4]={{0}};  // node node

  double complex trial1[6]={0};    // current
  double complex trial2[4]={0};    // charge


  /************************************
  Input A and b
  ************************************/
  //printf("Building Stiffness Matrix and trial vector ****** \n");
  
  double Edge_length[6]={0};            // edge length of each element

  /******************************
  dummy variables
  *******************************/
  double complex jce[6];
  double complex rhoe[4];

  double complex fe[4][4]={{0}}; 
  double complex feeps[4][4]={{0}}; 
  double complex ge[4][4]={{0}};

  double complex epsilone=0, mue=0; 
  double complex epsilone_xx=0;
  double complex epsilone_yy=0;
  double complex epsilone_zz=0;
  double complex mue_xx=0;
  double complex mue_yy=0;
  double complex mue_zz=0;

  /// for output the matrix: rot, mass, and element
  FILE *foutRot, *foutMass, *foutElem, *foutrhs;
  foutRot = fopen("save_data/matrix_MatRot3d.dat", "w");
  foutMass = fopen("save_data/matrix_MatMass3d.dat", "w");
  foutElem = fopen("save_data/matrix_matrix.dat", "w");
  foutrhs = fopen("save_data/vector_rhs.dat", "w");
  fprintf(foutRot,  "%d %d %d\n", N_element, 6, 6); 
  fprintf(foutMass, "%d %d %d\n", N_element, 6, 6); 
  fprintf(foutElem, "%d %d %d\n", N_element, 6, 6); 
  fprintf(foutrhs, "%d %d\n", N_element, 6); 

  for(e=0;e<N_element;e++){ 

     /**************************************
     global index of nodes of the element
    **************************************/ 

     i=Element[0+4*e]; 
     j=Element[1+4*e]; 
     m=Element[2+4*e];
     n=Element[3+4*e];

     /**************************************
     global index of edges  of the element
     including signs
     ***************************************/
     eg1=Element_Edge[0+6*e];  
     eg2=Element_Edge[1+6*e];     
     eg3=Element_Edge[2+6*e];  
     eg4=Element_Edge[3+6*e];  
     eg5=Element_Edge[4+6*e];  
     eg6=Element_Edge[5+6*e];  


     /**************************
     coordinates of each node 
     **************************/
     /*    node 1    ******   node 2    ******   node 3    ******   node 4 */
     xe[0]=Node[0+3*i]; xe[1]=Node[0+3*j]; xe[2]=Node[0+3*m]; xe[3]=Node[0+3*n];
     ye[0]=Node[1+3*i]; ye[1]=Node[1+3*j]; ye[2]=Node[1+3*m]; ye[3]=Node[1+3*n];
     ze[0]=Node[2+3*i]; ze[1]=Node[2+3*j]; ze[2]=Node[2+3*m]; ze[3]=Node[2+3*n];

     /***********************************
     coefficient of linear nodal element
     ************************************/
     ae[0]= xe[1]*(ye[2]*ze[3]-ze[2]*ye[3])-xe[2]*(ye[1]*ze[3]-ze[1]*ye[3])+xe[3]*(ye[1]*ze[2]-ze[1]*ye[2]);
     ae[1]=-xe[0]*(ye[2]*ze[3]-ze[2]*ye[3])+xe[2]*(ye[0]*ze[3]-ze[0]*ye[3])-xe[3]*(ye[0]*ze[2]-ze[0]*ye[2]);
     ae[2]= xe[0]*(ye[1]*ze[3]-ze[1]*ye[3])-xe[1]*(ye[0]*ze[3]-ze[0]*ye[3])+xe[3]*(ye[0]*ze[1]-ze[0]*ye[1]);
     ae[3]=-xe[0]*(ye[1]*ze[2]-ze[1]*ye[2])+xe[1]*(ye[0]*ze[2]-ze[0]*ye[2])-xe[2]*(ye[0]*ze[1]-ze[0]*ye[1]);

     be[0]=-(ye[2]*ze[3]-ze[2]*ye[3])+(ye[1]*ze[3]-ze[1]*ye[3])-(ye[1]*ze[2]-ze[1]*ye[2]);
     be[1]= (ye[2]*ze[3]-ze[2]*ye[3])-(ye[0]*ze[3]-ze[0]*ye[3])+(ye[0]*ze[2]-ze[0]*ye[2]);
     be[2]=-(ye[1]*ze[3]-ze[1]*ye[3])+(ye[0]*ze[3]-ze[0]*ye[3])-(ye[0]*ze[1]-ze[0]*ye[1]);
     be[3]= (ye[1]*ze[2]-ze[1]*ye[2])-(ye[0]*ze[2]-ze[0]*ye[2])+(ye[0]*ze[1]-ze[0]*ye[1]);
     //be[0]=-be[0]; be[1]=-be[1]; be[2]=-be[2]; be[3]=-be[3];

     ce[0]= (xe[2]*ze[3]-ze[2]*xe[3])-(xe[1]*ze[3]-ze[1]*xe[3])+(xe[1]*ze[2]-ze[1]*xe[2]);
     ce[1]=-(xe[2]*ze[3]-ze[2]*xe[3])+(xe[0]*ze[3]-ze[0]*xe[3])-(xe[0]*ze[2]-ze[0]*xe[2]);
     ce[2]= (xe[1]*ze[3]-ze[1]*xe[3])-(xe[0]*ze[3]-ze[0]*xe[3])+(xe[0]*ze[1]-ze[0]*xe[1]);
     ce[3]=-(xe[1]*ze[2]-ze[1]*xe[2])+(xe[0]*ze[2]-ze[0]*xe[2])-(xe[0]*ze[1]-ze[0]*xe[1]);

     de[0]=-(xe[2]*ye[3]-ye[2]*xe[3])+(xe[1]*ye[3]-ye[1]*xe[3])-(xe[1]*ye[2]-ye[1]*xe[2]);
     de[1]= (xe[2]*ye[3]-ye[2]*xe[3])-(xe[0]*ye[3]-ye[0]*xe[3])+(xe[0]*ye[2]-ye[0]*xe[2]);
     de[2]=-(xe[1]*ye[3]-ye[1]*xe[3])+(xe[0]*ye[3]-ye[0]*xe[3])-(xe[0]*ye[1]-ye[0]*xe[1]);
     de[3]= (xe[1]*ye[2]-ye[1]*xe[2])-(xe[0]*ye[2]-ye[0]*xe[2])+(xe[0]*ye[1]-ye[0]*xe[1]);
     //de[0]=-de[0]; de[1]=-de[1]; de[2]=-de[2]; de[3]=-de[3]; 

     Ve=(ae[0]+ae[1]+ae[2]+ae[3])/6.0;

     for(ii=0;ii<4;ii++){ 
        ae[ii]=ae[ii]/(6.0*Ve); 
        be[ii]=be[ii]/(6.0*Ve); 
        ce[ii]=ce[ii]/(6.0*Ve); 
        de[ii]=de[ii]/(6.0*Ve);
     }

     Vole=fabs(Ve);  

     
     jce[0]=Jc[eg1];  
     jce[1]=Jc[eg2];  
     jce[2]=Jc[eg3];  
     jce[3]=Jc[eg4];  
     jce[4]=Jc[eg5];  
     jce[5]=Jc[eg6];  
     rhoe[0]=rho[i]; 
     rhoe[1]=rho[j]; 
     rhoe[2]=rho[m]; 
     rhoe[3]=rho[n];

     if(fabs(creal(epsilon[i]+epsilon[j]+epsilon[m]+epsilon[n])-4)>1e-5){
       epsilone=epsilon[i];
       if(creal(epsilon[j])>creal(epsilone)){
         epsilone=epsilon[j];
       }
       if(creal(epsilon[m])>creal(epsilone)){
         epsilone=epsilon[m];
       }
       if(creal(epsilon[n])>creal(epsilone)){
         epsilone=epsilon[n];
       }
     }
     else{
       epsilone=(epsilon[i]+epsilon[j]+epsilon[m]+epsilon[n])/4.0;
     }
     mue=(mu[i]+mu[j]+mu[m]+mu[n])/4.0;

     epsilone_xx=epsilone;
     epsilone_yy=epsilone;
     epsilone_zz=epsilone;
     mue_xx=mue;
     mue_yy=mue;
     mue_zz=mue;
     

     //Edge_length  
     Edge_length[0]=Length_of_Edge[eg1];
     Edge_length[1]=Length_of_Edge[eg2];
     Edge_length[2]=Length_of_Edge[eg3];
     Edge_length[3]=Length_of_Edge[eg4];
     Edge_length[4]=Length_of_Edge[eg5];
     Edge_length[5]=Length_of_Edge[eg6];

     // fe \nabla Ni \cdot \nabla Nj
     for(ii=0;ii<4;ii++){
        for(jj=0;jj<4;jj++){
           fe[ii][jj]=be[ii]*be[jj]+ce[ii]*ce[jj]+de[ii]*de[jj];
           feeps[ii][jj]=be[ii]*be[jj]*epsilone_xx+ce[ii]*ce[jj]*epsilone_yy+de[ii]*de[jj]*epsilone_zz;
        }
     }    

     // ge Ni * Nj
     for(ii=0;ii<4;ii++){
        for(jj=0;jj<4;jj++){
           if(jj==ii){
             ge[ii][jj]=Vole/10.0;
           }
           else{
             ge[ii][jj]=Vole/20.0;
           }
        }
     }   


     // k1 curlA \cdot curlA  
     for(ii=0;ii<6;ii++){ 
        i1=Edge_end[ii][0];i2=Edge_end[ii][1];      // local index of end points of edge
        for(jj=0;jj<6;jj++){ 
           j1=Edge_end[jj][0]; j2=Edge_end[jj][1];  // local index of end points of edge
           ke1[ii][jj]=4.0*Edge_length[ii]*Edge_length[jj]*Vole*((ce[i1]*de[i2]-de[i1]*ce[i2])*(ce[j1]*de[j2]-de[j1]*ce[j2])+(de[i1]*be[i2]-be[i1]*de[i2])*(de[j1]*be[j2]-be[j1]*de[j2])+(be[i1]*ce[i2]-ce[i1]*be[i2])*(be[j1]*ce[j2]-ce[j1]*be[j2]));
           kemu1[ii][jj]=4.0*Edge_length[ii]*Edge_length[jj]*Vole*((ce[i1]*de[i2]-de[i1]*ce[i2])*(ce[j1]*de[j2]-de[j1]*ce[j2])/mue_xx+(de[i1]*be[i2]-be[i1]*de[i2])*(de[j1]*be[j2]-be[j1]*de[j2])/mue_yy+(be[i1]*ce[i2]-ce[i1]*be[i2])*(be[j1]*ce[j2]-ce[j1]*be[j2])/mue_zz);
        }
     }

     // 1.A. print real part
     for(ii = 0; ii < 6; ii++){
       for(jj = 0; jj < 6; jj++){
	 fprintf(foutRot, "%lf ", creal(ke1[ii][jj]));
       }
     }
     fprintf(foutRot, "\n");
     /* // 1. B. print imag part */
     /* for(ii = 0; ii < 6; ii++){ */
     /*   for(jj = 0; jj < 6; jj++){ */
     /* 	 fprintf(foutRot, "%lf ", cimag(ke1[ii][jj])); */
     /*   } */
     /* } */
     /* fprintf(foutRot, "\n"); */
     

     // k2  A \cdot \nabla Nj
     for(ii=0;ii<6;ii++){ 
        i1=Edge_end[ii][0];i2=Edge_end[ii][1];    // local index of end points of edge
        for(jj=0;jj<4;jj++){
           ke2[ii][jj]=Vole/4.0*Edge_length[ii]*(fe[i2][jj]-fe[i1][jj]);  
           keeps2[ii][jj]=Vole/4.0*Edge_length[ii]*(feeps[i2][jj]-feeps[i1][jj]);
        }
     } 

     // k3 A \cdot A  Mass matrix
     for(ii=0;ii<6;ii++){
        i1=Edge_end[ii][0];i2=Edge_end[ii][1];     // local index of end points of edge
        for(jj=0;jj<6;jj++){ 
           j1=Edge_end[jj][0]; j2=Edge_end[jj][1]; // local index of end points of edge
           ke3[ii][jj]=Edge_length[ii]*Edge_length[jj]*(ge[i1][j1]*fe[i2][j2]-ge[i1][j2]*fe[i2][j1]-ge[i2][j1]*fe[i1][j2]+ge[i2][j2]*fe[i1][j1]);
           keeps3[ii][jj]=Edge_length[ii]*Edge_length[jj]*(ge[i1][j1]*feeps[i2][j2]-ge[i1][j2]*feeps[i2][j1]-ge[i2][j1]*feeps[i1][j2]+ge[i2][j2]*feeps[i1][j1]);
        }
     } 

     // 2.A. print real part
     for(ii = 0; ii < 6; ii++){
       for(jj = 0; jj < 6; jj++){
	 fprintf(foutMass, "%lf ", creal(ke3[ii][jj]));
       }
     }
     fprintf(foutMass, "\n");
     /* // 2. B. print imag part */
     /* for(ii = 0; ii < 6; ii++){ */
     /*   for(jj = 0; jj < 6; jj++){ */
     /* 	 fprintf(foutMass, "%lf ", cimag(ke3[ii][jj])); */
     /*   } */
     /* } */
     /* fprintf(foutMass, "\n"); */
      

     //k4 \nabla Ni \cdot \nabla Nj
     for(ii=0;ii<4;ii++){
        for(jj=0;jj<4;jj++){
           ke4[ii][jj]=Vole*fe[ii][jj]; 
           keeps4[ii][jj]=Vole*feeps[ii][jj];
        } 
     } 

     // k5   
     for(ii=0;ii<4;ii++){
        for(jj=0;jj<6;jj++){  
           j1=Edge_end[jj][0]; j2=Edge_end[jj][1]; // local index of end points of edge 
           ke5[ii][jj]=Vole/4.0*Edge_length[jj]*(fe[ii][j2]-fe[ii][j1]); 
           keeps5[ii][jj]=Vole/4.0*Edge_length[jj]*(feeps[ii][j2]-feeps[ii][j1]);
        }
     } 

     // k6
     for(ii=0;ii<4;ii++){
        for(jj=0;jj<4;jj++){ 
           ke6[ii][jj]=ge[ii][jj]; 
        }
     } 
      

     // set up matrix  

     //A   
     double complex my_mat[6][6];
     double complex my_rhs[6];
     for(ii=0;ii<6;ii++){
        im=Element_Edge[ii+6*e];  // global index of this edge 
        if(im>=N_edge){ printf("wrong im\n"); break;}
        
        p_start=im*nz_row;
        isize=val_size[im];
        ik=p_start+val_size[im];

        for(jj=0;jj<6;jj++){
           jm=Element_Edge[jj+6*e];  // global index of this edge  
           if(jm>=N_edge){ printf("wrong jm\n"); break;}

           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+(kemu1[ii][jj]-omega*omega/(c_vacc*c_vacc)*keeps3[ii][jj]);
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+(kemu1[ii][jj]-omega*omega/(c_vacc*c_vacc)*keeps3[ii][jj]);
             col_ind_temp[ik]=jm;   
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];  
           }
	   
	   my_mat[ii][jj] = (kemu1[ii][jj]-omega*omega/(c_vacc*c_vacc)*keeps3[ii][jj]);
           
        }
  

        //*
        for(jj=0;jj<4;jj++){
           jm=Element[jj+4*e]+N_edge;  // global index of this node  

           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+(-csqrt(-1.0)*omega/c_vacc*keeps2[ii][jj]);
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+(-csqrt(-1.0)*omega/c_vacc*keeps2[ii][jj]);
             col_ind_temp[ik]=jm;  
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];
           }
        }//*/ 

	//* rhs for first equation
        trial1[ii]=0;
        for(jj=0;jj<6;jj++){   
           trial1[ii]=trial1[ii]+jce[jj]*ke3[ii][jj];
        }
        b_trial[im]=b_trial[im]+4.0*PI/c_vacc*trial1[ii];

	my_rhs[ii] = 4.0*PI/c_vacc*trial1[ii];

     } // finish first equation

     // 3.A. print real part of element matrix
     for(ii = 0; ii < 6; ii++){
       for(jj = 0; jj < 6; jj++){
	 fprintf(foutElem, "%lf ", creal(my_mat[ii][jj]));
       }
       fprintf(foutrhs, "%lf ", creal(my_rhs[ii]));
     }
     fprintf(foutElem, "\n");
     fprintf(foutrhs, "\n");
     // 3.B. print imag part
     for(ii = 0; ii < 6; ii++){
       for(jj = 0; jj < 6; jj++){
	 fprintf(foutElem, "%lf ", cimag(my_mat[ii][jj]));
       }
       fprintf(foutrhs, "%lf ", cimag(my_rhs[ii]));
     }
     fprintf(foutElem, "\n"); 
     fprintf(foutrhs, "\n");
     
 
     //phi
     for(ii=0;ii<4;ii++){
        im=Element[ii+4*e]+N_edge;      // global index of this node (put in second block)

        p_start=im*nz_row; 
        isize=val_size[im];
        ik=p_start+val_size[im];

        for(jj=0;jj<4;jj++){
           jm=Element[jj+4*e]+N_edge;   // global index of this node (put in second block) 

           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+gaugepara*(keeps4[ii][jj]);  
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+gaugepara*(keeps4[ii][jj]); 
             col_ind_temp[ik]=jm; 
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];
           }
        } 

        //*
        for(jj=0;jj<6;jj++){
           jm=Element_Edge[jj+6*e];    // global index of this edge 
           if(jm>=N_edge){ printf("wrong jm\n"); break;}

           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+(-csqrt(-1.0)*omega/c_vacc*keeps5[ii][jj]);
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+(-csqrt(-1.0)*omega/c_vacc*keeps5[ii][jj]);
             col_ind_temp[ik]=jm; 
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];
           }
        }//*/  


        /*
        trial2[ii]=0;
        for(jj=0;jj<4;jj++){
           trial2[ii]=trial2[ii]+rhoe[jj]*ke6[ii][jj];
        } 
        b_trial[im]=b_trial[im]+4.0*PI*trial2[ii];  
        //*/

        //*
        trial2[ii]=0;
        for(jj=0;jj<6;jj++){
           trial2[ii]=trial2[ii]+jce[jj]*ke5[ii][jj];
        } 
        b_trial[im]=b_trial[im]+4.0*PI*csqrt(-1.0)/omega*trial2[ii];  
        //*/   
   
     }  // row ii

  }  // finish element
     
  fclose(foutRot);
  fclose(foutMass);
  fclose(foutElem);
  fclose(foutrhs);
  
  double v1[3]={0}, v2[3]={0}, v3[3]={0};
  double De=0;
  double normal[3]={0};
  double complex wk=0;  
  double complex wk2=0;
  double complex wk1_xx=0;  
  double complex wk1_yy=0;  
  double complex wk1_zz=0;  
  double complex ke2d[3][3]={0};
  double complex fe2dwk[3][3]={0};
  double complex ke2dwk1[3][3]={0};
  double complex ke2dwk2[3][3]={0};

  int Face_Edge_End[3][3];
  Face_Edge_End[0][0]=0; Face_Edge_End[0][1]=1;
  Face_Edge_End[1][0]=0; Face_Edge_End[1][1]=2;
  Face_Edge_End[2][0]=1; Face_Edge_End[2][1]=2;
  
  /***************************************
  Assign Impedance BC
  1/mu (\nabla x E) x n + ik1 (E x n) x n = 0
  
  E x n x n = -E + (n \codt E) n

  //d\phi/dn - ik2 \phi = 0  or d\phi/dn=0

  k1=1/eta; k2=omega/c_vacc;
  eta=sqrt(mu/epsilon)
  ****************************************/
  //printf("Assigning IBC\n");
  for(e=N_triangle_interface;e<N_triangle;e++){
     // three nodes 
     i=Triangle[0+3*e]; 
     j=Triangle[1+3*e]; 
     m=Triangle[2+3*e]; 

     // three edges 
     eg1=Triangle_Edge[0+3*e]; 
     eg2=Triangle_Edge[1+3*e]; 
     eg3=Triangle_Edge[2+3*e]; 

     // Edge_length  
     Edge_length[0]=Length_of_Edge[eg1];
     Edge_length[1]=Length_of_Edge[eg2];
     Edge_length[2]=Length_of_Edge[eg3];
  
     // coordinates of each node
     //    node 1             node 2             node 3    
     xe[0]=Node[0+3*i]; xe[1]=Node[0+3*j]; xe[2]=Node[0+3*m];  
     ye[0]=Node[1+3*i]; ye[1]=Node[1+3*j]; ye[2]=Node[1+3*m]; 
     ze[0]=Node[2+3*i]; ze[1]=Node[2+3*j]; ze[2]=Node[2+3*m];  

     // vectors of node1 to node 2; and node1 to node 3 
     v1[0]=(xe[1]-xe[0]); 
     v1[1]=(ye[1]-ye[0]); 
     v1[2]=(ze[1]-ze[0]); 
     v2[0]=(xe[2]-xe[0]); 
     v2[1]=(ye[2]-ye[0]); 
     v2[2]=(ze[2]-ze[0]);

     // aera of this triangle 
     De=0.5*sqrt((v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2])*(v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2])-(v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])*(v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]));

     // normal to this triangle 
     normal[0]=(v1[1]*v2[2]-v2[1]*v1[2]); 
     normal[1]=(v2[0]*v1[2]-v1[0]*v2[2]); 
     normal[2]=(v1[0]*v2[1]-v1[1]*v2[0]);

     // node 4  
     ie=Triangle_Element[1+2*e]; if(ie<0 || ie>=N_element){ printf("wrong triangle-element relation\n");}
     for(ii=0;ii<4;ii++){
        if(!(Element[ii+4*ie]==i || Element[ii+4*ie]==j || Element[ii+4*ie]==m)){
          n=Element[ii+4*ie]; break;
        }
     } 
     xe[3]=Node[0+3*n]; 
     ye[3]=Node[1+3*n]; 
     ze[3]=Node[2+3*n]; 
     // vector of node 1 to node 4  
     v3[0]=(xe[3]-xe[0]); 
     v3[1]=(ye[3]-ye[0]); 
     v3[2]=(ze[3]-ze[0]); 
     // outward normal 
     if((v1[0]*v3[0]+v1[1]*v3[1]+v1[2]*v3[2])>0){
       normal[0]=-normal[0]; 
       normal[1]=-normal[1]; 
       normal[2]=-normal[2];
     } 
     maxm=sqrt(normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2]);
     normal[0]=normal[0]/maxm; 
     normal[1]=normal[1]/maxm; 
     normal[2]=normal[2]/maxm;
     //printf("%lf  %lf  %lf\n",normal[0], normal[1], normal[2]);


     // coefficient of linear nodal element  
     ae[0]= xe[1]*(ye[2]*ze[3]-ze[2]*ye[3])-xe[2]*(ye[1]*ze[3]-ze[1]*ye[3])+xe[3]*(ye[1]*ze[2]-ze[1]*ye[2]);
     ae[1]=-xe[0]*(ye[2]*ze[3]-ze[2]*ye[3])+xe[2]*(ye[0]*ze[3]-ze[0]*ye[3])-xe[3]*(ye[0]*ze[2]-ze[0]*ye[2]);
     ae[2]= xe[0]*(ye[1]*ze[3]-ze[1]*ye[3])-xe[1]*(ye[0]*ze[3]-ze[0]*ye[3])+xe[3]*(ye[0]*ze[1]-ze[0]*ye[1]);
     ae[3]=-xe[0]*(ye[1]*ze[2]-ze[1]*ye[2])+xe[1]*(ye[0]*ze[2]-ze[0]*ye[2])-xe[2]*(ye[0]*ze[1]-ze[0]*ye[1]);

     be[0]=-(ye[2]*ze[3]-ze[2]*ye[3])+(ye[1]*ze[3]-ze[1]*ye[3])-(ye[1]*ze[2]-ze[1]*ye[2]);
     be[1]= (ye[2]*ze[3]-ze[2]*ye[3])-(ye[0]*ze[3]-ze[0]*ye[3])+(ye[0]*ze[2]-ze[0]*ye[2]);
     be[2]=-(ye[1]*ze[3]-ze[1]*ye[3])+(ye[0]*ze[3]-ze[0]*ye[3])-(ye[0]*ze[1]-ze[0]*ye[1]);
     be[3]= (ye[1]*ze[2]-ze[1]*ye[2])-(ye[0]*ze[2]-ze[0]*ye[2])+(ye[0]*ze[1]-ze[0]*ye[1]);
     //be[0]=-be[0]; be[1]=-be[1]; be[2]=-be[2]; be[3]=-be[3];

     ce[0]= (xe[2]*ze[3]-ze[2]*xe[3])-(xe[1]*ze[3]-ze[1]*xe[3])+(xe[1]*ze[2]-ze[1]*xe[2]);
     ce[1]=-(xe[2]*ze[3]-ze[2]*xe[3])+(xe[0]*ze[3]-ze[0]*xe[3])-(xe[0]*ze[2]-ze[0]*xe[2]);
     ce[2]= (xe[1]*ze[3]-ze[1]*xe[3])-(xe[0]*ze[3]-ze[0]*xe[3])+(xe[0]*ze[1]-ze[0]*xe[1]);
     ce[3]=-(xe[1]*ze[2]-ze[1]*xe[2])+(xe[0]*ze[2]-ze[0]*xe[2])-(xe[0]*ze[1]-ze[0]*xe[1]);

     de[0]=-(xe[2]*ye[3]-ye[2]*xe[3])+(xe[1]*ye[3]-ye[1]*xe[3])-(xe[1]*ye[2]-ye[1]*xe[2]);
     de[1]= (xe[2]*ye[3]-ye[2]*xe[3])-(xe[0]*ye[3]-ye[0]*xe[3])+(xe[0]*ye[2]-ye[0]*xe[2]);
     de[2]=-(xe[1]*ye[3]-ye[1]*xe[3])+(xe[0]*ye[3]-ye[0]*xe[3])-(xe[0]*ye[1]-ye[0]*xe[1]);
     de[3]= (xe[1]*ye[2]-ye[1]*xe[2])-(xe[0]*ye[2]-ye[0]*xe[2])+(xe[0]*ye[1]-ye[0]*xe[1]);
     //de[0]=-de[0]; de[1]=-de[1]; de[2]=-de[2]; de[3]=-de[3]; 

     Ve=(ae[0]+ae[1]+ae[2]+ae[3])/6.0;

     for(ii=0;ii<4;ii++){ 
        ae[ii]=ae[ii]/(6.0*Ve); 
        be[ii]=be[ii]/(6.0*Ve); 
        ce[ii]=ce[ii]/(6.0*Ve); 
        de[ii]=de[ii]/(6.0*Ve);
     }

     Vole=fabs(Ve);  

     //epsilone=min2(min2(cabs(epsilon[i]),cabs(epsilon[j])),cabs(epsilon[m]));
     epsilone=(epsilon[i]+epsilon[j]+epsilon[m])/3.0;
     mue=(mu[i]+mu[j]+mu[m])/3.0;
     wk=csqrt(epsilone/mue); 
     wk2=omega/c_vacc;
     wk1_xx=wk; 
     wk1_yy=wk;
     wk1_zz=wk;

     // int Ni Nj on the face 
     for(ii=0;ii<3;ii++){
        for(jj=0;jj<3;jj++){
           if(jj==ii){
              ke2d[ii][jj]=De/6.0;
           }
           else{
              ke2d[ii][jj]=De/12.0;
           }
        }
     }

     // \nabla Ni x n \cdot \nalba Nj x n on face  
     for(ii=0;ii<3;ii++){
        for(jj=0;jj<3;jj++){
           fe2dwk[ii][jj]=(normal[2]*ce[ii]-normal[1]*de[ii])*(normal[2]*ce[jj]-normal[1]*de[jj])*wk1_xx + (normal[0]*de[ii]-normal[2]*be[ii])*(normal[0]*de[jj]-normal[2]*be[jj])*wk1_yy + (normal[1]*be[ii]-normal[0]*ce[ii])*(normal[1]*be[jj]-normal[0]*ce[jj])*wk1_zz;
        }
     }

     // int Ni x n \cdot Nj x n on face
     for(ii=0;ii<3;ii++){
        i1=Face_Edge_End[ii][0]; i2=Face_Edge_End[ii][1];
        for(jj=0;jj<3;jj++){
           j1=Face_Edge_End[jj][0]; j2=Face_Edge_End[jj][1];
           ke2dwk1[ii][jj]=Edge_length[ii]*Edge_length[jj]*(ke2d[i1][j1]*fe2dwk[i2][j2]-ke2d[i1][j2]*fe2dwk[i2][j1]-ke2d[i2][j1]*fe2dwk[i1][j2]+ke2d[i2][j2]*fe2dwk[i1][j1]);
        }
     }

     // int \nabla Ni x n \cdot Nj x n on face
     for(ii=0;ii<3;ii++){
        i1=Face_Edge_End[ii][0]; i2=Face_Edge_End[ii][1];
        for(jj=0;jj<3;jj++){
           j1=Face_Edge_End[jj][0]; j2=Face_Edge_End[jj][1];
           ke2dwk2[ii][jj]= De/3.0*Edge_length[jj]*(fe2dwk[ii][j2]-fe2dwk[ii][j1]); 
        }
     }
      
     
     //A 
     for(ii=0;ii<3;ii++){
        im=Triangle_Edge[ii+3*e];  
        p_start=im*nz_row; 
        isize=val_size[im];
        ik=p_start+val_size[im];
      
        //A
        for(jj=0;jj<3;jj++){
           jm=Triangle_Edge[jj+3*e];
           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+(-csqrt(-1)*c_vacc/omega*wk2*wk2*ke2dwk1[ii][jj]);
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+(-csqrt(-1)*c_vacc/omega*wk2*wk2*ke2dwk1[ii][jj]);
             col_ind_temp[ik]=jm; 
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];
           }
        }  
      
        //phi
        for(jj=0;jj<3;jj++){
           jm=Triangle[jj+3*e]+N_edge;
           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+(-csqrt(-1)*c_vacc/omega*csqrt(-1)*wk2*ke2dwk2[jj][ii]);
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+(-csqrt(-1)*c_vacc/omega*csqrt(-1)*wk2*ke2dwk2[jj][ii]);
             col_ind_temp[ik]=jm; 
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];
           }
        }  
     }  


     //phi
     for(ii=0;ii<3;ii++){
        im=Triangle[ii+3*e]+N_edge;  
        p_start=im*nz_row; 
        isize=val_size[im];
        ik=p_start+val_size[im];
        
        //A 
        for(jj=0;jj<3;jj++){
           jm=Triangle_Edge[jj+3*e];
           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+(c_vacc*c_vacc/omega/omega*wk2*wk2*ke2dwk2[ii][jj]);
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+(c_vacc*c_vacc/omega/omega*wk2*wk2*ke2dwk2[ii][jj]);
             col_ind_temp[ik]=jm; 
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];
           }
        } 
        
        //phi 
        for(jj=0;jj<3;jj++){
           jm=Triangle[jj+3*e]+N_edge;
           icheck=0;
           for(ix=p_start;ix<p_start+val_size[im];ix++){
              if(jm==col_ind_temp[ix]){
                val_temp[ix]=val_temp[ix]+(c_vacc*c_vacc/omega/omega*csqrt(-1)*wk2*fe2dwk[ii][jj])*De;
                icheck=1;
                break;
              }
           }
           if(icheck==0){
             val_temp[ik]=val_temp[ik]+(c_vacc*c_vacc/omega/omega*csqrt(-1)*wk2*fe2dwk[ii][jj])*De;
             col_ind_temp[ik]=jm; 
             val_size[im]=val_size[im]+1; ik=p_start+val_size[im];
           }
        } 
     }  
  }  

  // output sparse matix data
  //  write_txt2(col_ind_temp, val_temp, val_size,
  //	     Msize, nz_row, "spmat_pre_sort.txt");

  // sorting col_ind_temp
  double complex *val_sort=(double complex *)malloc(sizeof(double complex));
  int *ind_sort=(int *)malloc(sizeof(int));
  val_sort[0]=0; ind_sort[0]=0;
  free(val_sort); free(ind_sort);
  int size_sort=0;

  for(ik=0;ik<Msize;ik++){
     p_start=ik*nz_row;
     size_sort=val_size[ik];

     val_sort=(double complex *)malloc(size_sort*sizeof(double complex));
     ind_sort=(int *)malloc(size_sort*sizeof(int));

     for(im=0;im<size_sort;im++){
        val_sort[im]=val_temp[p_start+im];
        ind_sort[im]=col_ind_temp[p_start+im];
        if(ind_sort[im]<0){ printf("wrong col index\n"); getchar();}
     }

     quicksort(val_sort,ind_sort,0,size_sort-1);
     for(im=0;im<size_sort;im++){
        val_temp[p_start+im]=val_sort[im];
        col_ind_temp[p_start+im]=ind_sort[im];
        if(ind_sort[im]<0){ printf("wrong col index2\n"); getchar();}
     }
     free(val_sort); free(ind_sort);
  }


  //    write_txt2(col_ind_temp, val_temp, val_size,
  //	     Msize, nz_row, "spmat_post_sort.txt");

  for(ik=0;ik<Msize;ik++){
     for(im=ik*nz_row;im<ik*nz_row+val_size[ik]-1;im++){
        if(col_ind_temp[im] >= col_ind_temp[im+1]){ 
	  printf("wrong col_ind in row %d col %d.\n",
		 ik, col_ind_temp[im]); 
	  //	  return 0;
	}
     }
     for(im=ik*nz_row;im<ik*nz_row+val_size[ik];im++){ 
        if(col_ind_temp[im]==-1){ 
	  printf("wrong col_ind 2\n"); 
	  return 0;}
     }
  }
  
  for(ik=0;ik<Msize;ik++){
     icheck=0;
     p_start=ik*nz_row; p_end=p_start+val_size[ik];
     for(im=p_start;im<p_end;im++){
        if(col_ind_temp[im]==ik && cabs(val_temp[im])>1.0e-12){icheck=1;}
     }

     if(icheck==0){ printf("wrong matrix\n"); getchar();}
  } 
  /**********************
  Boundary Condtion
  ***********************/ 
  //printf("Assigning DBC \n"); 
  
  int *checkBC_Av=(int *)malloc((N_edge+N_node)*sizeof(int));
  for(ix=0;ix<(N_edge+N_node);ix++){
     checkBC_Av[ix]=0;
  }
  /*
  //for(e=0;e<N_triangle;e++){
  for(e=0;e<0;e++){
     for(ii=0;ii<3;ii++){ 
       
        im=Triangle_Edge[ii+3*e];  
  
        if(checkBC_Av[im]==0){ 
          b_trial[im]=0;//A_exact[im];  
          checkBC_Av[im]=1;

          p_start=im*nz_row; p_end=p_start+val_size[im];  
        
          for(ik=p_start;ik<p_end;ik++){ 
             jm=col_ind_temp[ik];
             if(jm==im){
               val_temp[ik]=1; 
             }
             else{
               val_temp[ik]=0; 
             } 

             // find (jm, im) entry 
             if(jm!=im){
               for(jj=jm*nz_row;jj<jm*nz_row+val_size[jm];jj++){
                  if(col_ind_temp[jj]==im){
                    b_trial[jm]=b_trial[jm]-val_temp[jj]*b_trial[im]; 
                    val_temp[jj]=0; break;
                  }
               }
             }   
          } 
        }

          
        im=Triangle[ii+3*e]+N_edge;    
  
        if(checkBC_Av[im]==0){ 
          b_trial[im]=0;//phi_exact[im-N_edge];   
          checkBC_Av[im]=1;

          p_start=im*nz_row; p_end=p_start+val_size[im];  

          for(ik=p_start;ik<p_end;ik++){ 
             jm=col_ind_temp[ik];  
             if(jm==im){
               val_temp[ik]=1; 
             }
             else{
               val_temp[ik]=0; 
             } 

             // find (jm, im) entry  
             if(jm!=im){
               for(jj=jm*nz_row;jj<jm*nz_row+val_size[jm];jj++){
                  if(col_ind_temp[jj]==im){
                    b_trial[jm]=b_trial[jm]-val_temp[jj]*b_trial[im]; 
                    val_temp[jj]=0; break;
                  }
               }
             } 
          }
        }  
     } 
  }
  //*/  
   
      
  //Collect same terms of each row  

  // Non zeros terms
  ik=0;
  for(ix=0;ix<Msize*nz_row;ix++){  
     if(cabs(val_temp[ix])>0.0){ ik=ik+1;}
  }
  row_ptr[Msize]=ik;   

  int Total_Nz=ik; 
  //printf("Total_Nz = %d\n", Total_Nz); getchar();

  double complex *val=(double complex *)malloc(Total_Nz*sizeof(double complex));
  int *col_ind=(int *)malloc(Total_Nz*sizeof(int)); 
  int pos=0; 

  
  // start of each row
  pos=0;
  for(ix=0;ix<Msize;ix++){ 
     row_ptr[ix]=pos; 
     p_start=ix*nz_row; p_end=p_start+nz_row-1;  
     for(ik=p_start;ik<=p_end;ik++){
        if(cabs(val_temp[ik])>0.0){
          val[pos]=val_temp[ik];
          col_ind[pos]=col_ind_temp[ik];
          pos=pos+1; 
        }
     }
  }  

  free(val_temp); free(col_ind_temp);   free(val_size); free(checkBC_Av);


  //printf("Building stiffness matrix and trial vector DONE! ****** \n\n");

  //printf("\t Solving linear system ****** \n");

  double complex *sol=(double *)malloc(Msize*sizeof(double complex));
  for(i=0;i<Msize;i++) sol[i]=0;

  //  BiCGSTAB_precond_CRS(val, col_ind, row_ptr, b_trial, sol, Msize); 
  QMR_precond_CRS(val, col_ind, row_ptr, b_trial, sol, Msize, 500, 1e-8); 
  
  /****************
  Save data
  *****************/ 
  for(e=0;e<N_edge;e++){ 
     A_EM[e] = sol[e];  
  } 
  for(e=0;e<N_node;e++){  
     phi[e] = sol[e + N_edge];
  }

  free(b_trial);  free(sol); 
  free(val); free(col_ind); free(row_ptr); 
   

  return 0;
}
